#include "stdafx.h"
#include "DivisionAdjuster.h"
#include <algorithm>

static const float kSmallFraction = 0.001f;

static inline bool Check2Pi(float a, float b)
{
	return ::fabs(b - a - DirectX::XM_2PI) < 0.001;
}
#define CHECK_ANGLES assert(Check2Pi(mAngles[0], mAngles[mCount]))

static 
float CubicCurveMaximumPosition(float xmax, float deltaY, float yD, float yDD)
{
	float ydL = yD * xmax;
	float yddLL = yDD * xmax * xmax;
	float minusA = -deltaY + ydL + 0.5f * yddLL;
	if (::fabs(minusA) < 0.001f * fabs(yddLL))
	{
		// vertex of parabola
		return ::fabs(yDD) > 0 ? (-yD / yDD) : 0;
	}
	if (::fabs(minusA) > 0)
	{
		float D = yddLL * yddLL + 12 * minusA * ydL;
		if (D >= 0)
		{
			float t = xmax * (yddLL + ::sqrt(D)) / (6 * minusA);
			if (yD * t + 0.5f * yDD * t * t - minusA * t * t * t > deltaY)
			{
				return min(t, 1.5f * xmax);
			}
		}
	}
	// maximum at the end of interval
	return deltaY > 0 ? xmax : 0;
}

static
float FindRootOfCubicEquation(float a0, float a1, float a2, float a3)
{
	float r = sqrt(a0 / a2);
	float a = a1 * r / a0;
	float b = a3 * r / a2;
	// scaled to r, the equation looks like:
	// 1 + a * t + t * t + b * t * t * t = 0;
	// result = r * t;

	// x^3 + 3px + q = 0; t = x - delta
	float delta = 1.0f / (3 * b);
	float p3 = delta * (a - delta);
	float q = delta * (2 * delta  * delta + 3 - 3 * a * delta);

	float dd = q * q + 4 * p3 * p3 * p3;
	assert(dd > 0);
	float omegaCube = 0.5f * (sqrt(dd) - q);
	float omega = 0;
	if (omegaCube > 0)
	{
		omega = ::pow(omegaCube, 1.0f / 3);
	}
	else
	{
		omega = -::pow(-omegaCube, 1.0f / 3);
	}
	float x = omega - p3 / omega;
	float t = x - delta;

	return r * t;
}


DivisionAdjuster::DivisionAdjuster(const Fourier& inFourier)
	: mFourier(inFourier), mAngles(), mColors(), mIntegrals(),
	mF(), mDF()
{
}

void DivisionAdjuster::ResetWithAngles(int inCount, const float* inAngles)
{
	assert(inCount > 1);
	mCount = inCount;
	mAngles.assign(inAngles, inAngles + inCount);
	mAngles.push_back(mAngles[0] + DirectX::XM_2PI);
	SpecialEquationSolver::Resize(inCount);
	ResetAllColors();
	CHECK_ANGLES;
}

void DivisionAdjuster::ApplyDeltas()
{
	CHECK_ANGLES;
	for (int i = mCount; i--;)
	{
		mAngles[i] += SpecialEquationSolver::GetX(i);
	}
	if (mAngles[0] < 0)
	{
		mAngles[0] += DirectX::XM_2PI;
		RotateOnlyAnglesDown();
	}
	else if (mAngles[mCount - 1] >= DirectX::XM_2PI)
	{
		mAngles[mCount - 1] -= DirectX::XM_2PI;
		RotateOnlyAnglesUp();
	}

	mAngles[mCount] = mAngles[0] + DirectX::XM_2PI;
	ResetAllColors();
	CHECK_ANGLES;
}

bool DivisionAdjuster::RawAdjustAllAngles()
{
	bool result = false;
	for (int i = 0; i < mCount; ++i)
	{
		CHECK_ANGLES;
		result |= RawAdjustAngle(i);
		CHECK_ANGLES;
	}
	return result;
}

bool DivisionAdjuster::RawAdjustAnglesWithWrongSignOfSecondDerivative()
{
	bool result = false;
	for (int i = 0; i < mCount; ++i)
	{
		if (SpecialEquationSolver::mDiagonal[i] >= 0)
		{
			result |= RawAdjustAngle(i);
			if (SpecialEquationSolver::mDiagonal[i] >= 0)
			{
				// ?
				result |= RawAdjustAngle(i);
			}
		}
	}
	return result;
}


bool DivisionAdjuster::RawAdjustAngle(int inIndex)
{
	CHECK_ANGLES;
	using namespace DirectX;
	mTemporaryNextAngle = mAngles[inIndex + 1];
	mTemporaryPrevAngle = (inIndex >= 1 ? mAngles[inIndex - 1] : 
		mAngles[inIndex + mCount - 1] - XM_2PI);
	float angleInterval = mTemporaryNextAngle - mTemporaryPrevAngle;

	int prevIndex = (inIndex + mCount - 1) % mCount;
	int nextIndex = (inIndex + 1) % mCount;
	mTemporaryPrevIntegral = mIntegrals[prevIndex];
	mTemporaryNextIntegral = mIntegrals[nextIndex];
	
	int sampleCount = std::max<int>(2, lround(3 * mFourier.GetDegree() 
		* angleInterval / XM_2PI));
	
	const float deltaA = angleInterval / sampleCount;
	auto deltaIntegral = mTemporaryNextIntegral - mTemporaryPrevIntegral;
	const float endValue = XMVector3Dot(deltaIntegral, deltaIntegral).m128_f32[0]
		/ angleInterval;
	float prevH[2] = { endValue, endValue };
	float leftFlankValue, rightFlankValue;

	float maxH = endValue;
	int indexOfMaxValue = -1;
	float angleOfMaxH = mTemporaryPrevAngle;
	float h = endValue;

	float angle = mTemporaryPrevAngle;
	float bestGuessAngle = mAngles[inIndex];
	for (int i = 1; i < sampleCount;)
	{
		angle += deltaA;
		float h = CalculatePartialScore(angle);

		if (h > maxH)
		{
			maxH = h;
			indexOfMaxValue = i;
			angleOfMaxH = angle;
		}
		else if (indexOfMaxValue + 1 == i)
		{
			leftFlankValue = prevH[indexOfMaxValue & 1];
			rightFlankValue = h;
		}

		prevH[(++i) & 1] = h;
	}
	
	if (indexOfMaxValue + 1 == sampleCount)
	{
		leftFlankValue = prevH[indexOfMaxValue & 1];
		rightFlankValue = endValue;
	}

	angle = mAngles[inIndex];
	float currentH = CalculatePartialScore(angle);
	if (currentH < maxH)
	{
		// calculate derivative
		XMVECTOR integral = mFourier.IntegralWithoutConstantAtAngle(angleOfMaxH);
		XMVECTOR ck = (mTemporaryNextIntegral - integral) / 
			(mTemporaryNextAngle - angleOfMaxH);
		XMVECTOR ck1 = (integral - mTemporaryPrevIntegral) /
			(angleOfMaxH - mTemporaryPrevAngle);
		XMVECTOR f = mFourier.ColorComponentsWithoutConstantAtAngle(angleOfMaxH);
		auto d = ck - f;
		auto d1 = f - ck1;
		float derivative = XMVector3Dot(d, d).m128_f32[0] - XMVector3Dot(d1, d1).m128_f32[0];
		if (derivative > 0)
		{
			if (angle <= angleOfMaxH || angle > angleOfMaxH + deltaA)
			{
				angle = angleOfMaxH + deltaA;
				currentH = rightFlankValue;
			}
		}
		else
		{
			if (angle >= angleOfMaxH || angle < angleOfMaxH - deltaA)
			{
				angle = angleOfMaxH - deltaA;
				currentH = leftFlankValue;
			}
		}
		float L = angle - angleOfMaxH;
		float dv = derivative * L;
		bestGuessAngle = angleOfMaxH + 0.5f * dv * L / (maxH - currentH + dv);
		float bestH = CalculatePartialScore(bestGuessAngle);
		if (bestH < maxH)
		{
			bestGuessAngle = angleOfMaxH;
		}
	}
	else
	{
		float derivative = SpecialEquationSolver::mB[inIndex];
		float endAngle = 0;
		float endH = 0;
		float position = (angle - angleOfMaxH) / deltaA;
		if (::fabs(angle - angleOfMaxH) <= deltaA)
		{
			double epsilon = 0.01 * deltaA;
			if (derivative > 0)
			{
				if (angle + epsilon < angleOfMaxH)
				{
					endAngle = angleOfMaxH;
					endH = maxH;
				}
				else
				{
					endAngle = angleOfMaxH + deltaA;
					endH = rightFlankValue;
				}
			}
			else
			{
				if (angle > angleOfMaxH + epsilon)
				{
					endAngle = angleOfMaxH;
					endH = maxH;
				}
				else
				{
					endAngle = angleOfMaxH - deltaA;
					endH = leftFlankValue;
				}
			}
		}
		else
		{
			if (derivative > 0)
			{
				endAngle = min(angle + 0.5f * deltaA, mTemporaryNextAngle);
			}
			else
			{
				endAngle = std::max(angle - 0.5f * deltaA, mTemporaryPrevAngle);
			}
			endH = CalculatePartialScore(endAngle);
		}

		if (endH > currentH)
		{
			bestGuessAngle = endAngle;
		}
		else
		{
			float betterAngle = angle + ::CubicCurveMaximumPosition(endAngle - angle,
				endH - currentH, derivative, SpecialEquationSolver::mDiagonal[inIndex]);

			float h = CalculatePartialScore(betterAngle);
			if (h >= currentH)
			{
				bestGuessAngle = betterAngle;
			}
			else
			{
				// no change
				return false;
			}
		}
	}


	/////////

	int indexOfChangedAngle = inIndex;

	if (inIndex == 0 && bestGuessAngle < 0)
	{
		mAngles[0] = bestGuessAngle + XM_2PI;
		RotateDown();
		indexOfChangedAngle = mCount - 1;
		mAngles[mCount] = mAngles[0] + DirectX::XM_2PI;
	}
	else if (inIndex == mCount - 1 && bestGuessAngle >= XM_2PI)
	{
		mAngles[mCount - 1] = bestGuessAngle - XM_2PI;
		RotateUp();
		indexOfChangedAngle = 0;
		mAngles[mCount] = mAngles[0] + DirectX::XM_2PI;
	}
	else
	{
		mAngles[inIndex] = bestGuessAngle;
	}
	mAngles[mCount] = mAngles[0] + DirectX::XM_2PI;
	CHECK_ANGLES;
	ResetColorAtIndex(indexOfChangedAngle);
	CHECK_ANGLES;


	return true;
}

void DivisionAdjuster::OutputAngles(size_t inN, float* outAngles) const
{
	// output also the additional last angle (= first + 2pi)
	assert(mAngles.size() == inN + 1);
	::memcpy(outAngles, &mAngles[0], (inN + 1) * sizeof(float));
}

void DivisionAdjuster::OutputColors(size_t inN, DirectX::XMVECTOR* outColors) const
{
	assert(mColors.size() == inN);
	using namespace DirectX;
	auto constantCoefficient = mFourier.GetConstantCoefficient();
	for (size_t i = 0; i < inN; ++i)
	{
		outColors[i] = mColors[i] + constantCoefficient;
	}
}


int DivisionAdjuster::GetIndexOfAnglesOverlap() const
{
	float greaterAngle = mAngles[mCount] + SpecialEquationSolver::GetX(0);
	for (int i = mCount; i--;)
	{
		float angle = mAngles[i] + SpecialEquationSolver::GetX(i);
		if (angle >= greaterAngle)
		{
			return i;
		}
		greaterAngle = angle;
	}
	return kInvalidIndex;
}

// some custom function, it should be chosen empirically so that
// the number of "Few discrete colors" corresponds to intuitive expectation
static 
float ConvertScore(float score, int n)
{
	return ::log(score * n);
}

int DivisionAdjuster::ReduceNumberOfSegments()
{
	if (mCount <= 2)
	{
		return mCount;
	}
	ScoreAndMaskVector scoresAndMasks;
	CalculateOptimalSubdivisions(scoresAndMasks);
	int scoresAndMasksSize = scoresAndMasks.size();
	assert(scoresAndMasksSize >= 2);

	std::vector<FloatVector> anglesPerNumberOfSegments(scoresAndMasksSize - 1);
	DivisionAdjuster adjuster(mFourier);
	float higherScore = scoresAndMasks.back().score;
	// the following cycle will adjust the "score" member in each item of the vector;
	// the masks are used for calculation of angles into another vector
	for (int i = scoresAndMasksSize - 1; i--;)
	{
		FloatVector& angles = anglesPerNumberOfSegments[i];
		angles.reserve(i + 3);
		int index = 0;
		for (MaskType mask = scoresAndMasks[i].mask; 0 != mask; 
			mask >>= 1, ++index)
		{
			if (0 != (mask & 1))
			{
				angles.push_back(mAngles[index]);
			}
		}
		
		int count = angles.size();
		adjuster.ResetWithAngles(count, &angles[0]);

		// the in-place adjustments of scores in the vector scoresAndMasks.
		float& currentScore = scoresAndMasks[i].score;
		adjuster.RawAdjustTowardsHigherScore(currentScore, higherScore);

		angles.push_back(0);
		adjuster.OutputAngles(count, &angles[0]);
		higherScore = currentScore;
	}


	float scoreLimit = mFourier.IntegralOfSquareAroundCircle();
	float maxGap = 0;
	int indexOfMaxGap = kInvalidIndex;

	float previousSmallestValue = ::ConvertScore(scoreLimit, 1);	
	for (int i = 0; i < scoresAndMasksSize; ++i)
	{
		const float value = ::ConvertScore(scoreLimit - scoresAndMasks[i].score,
			i + 2);
		const float gap = previousSmallestValue - value;
		if (gap > maxGap)
		{
			maxGap = gap;
			indexOfMaxGap = i;
		}
		if (value < previousSmallestValue)
		{
			previousSmallestValue = value;
		}
	}

	if (kInvalidIndex == indexOfMaxGap || scoresAndMasksSize - 1 == indexOfMaxGap)
	{
		return mCount;
	}

	if (0 <= indexOfMaxGap && indexOfMaxGap < scoresAndMasksSize - 1)
	{
		FloatVector& angles = anglesPerNumberOfSegments[indexOfMaxGap];
		ResetWithAngles(angles.size() - 1, &angles[0]);
		ResetAllColors();
	}

	return mCount;
}

bool DivisionAdjuster::MakeGradientAscent()
{
	float vv = 0;
	float va = 0;
	float aa = 0;
	float faa = 0;
	float prevA = 0;
	FloatVector a(mCount);

	const float kScaleDown = 1.0f / 1024;

	for (int i = 0; i < mCount; ++i)
	{
		int prevIndex = (i + mCount - 1) % mCount;
		int nextIndex = (i + 1) % mCount;

		// using variables from the base class SpecialEquationSolver
		float v = kScaleDown * mB[i];
		vv += v * v;
		a[i] = kScaleDown * kScaleDown * (mDiagonal[i] * mB[i] +
			mB[prevIndex] * mNonDiagonal[prevIndex] +
			mB[nextIndex] * mNonDiagonal[i]);
		va += v * a[i];
		aa += a[i] * a[i];
		faa += kScaleDown * (mDiagonal[i] * a[i] * a[i] +
			2 * mNonDiagonal[prevIndex] * a[i] * prevA);
		prevA = a[i];
	}
	faa += kScaleDown * 2 * mNonDiagonal[mCount - 1] * a[mCount - 1] * a[0];


	if (faa >= 0)
	{
		return false;
	}

	// calculate step in the direction of gradient curve;
	float t = ::FindRootOfCubicEquation(vv, 2 * va, 1.5f * aa,
		0.5f * faa);

	float score = CalculateScore();

	for (int tries = 3; tries--; t *= 0.7f)
	{
		FloatVector angles = mAngles;

		float halfSquaredT = 0.5f * t * t;
		for (int i = mCount; i--;)
		{
			angles[i] += t * kScaleDown * SpecialEquationSolver::mB[i] +
				halfSquaredT * a[i];
		}

		angles[mCount] = angles[0] + DirectX::XM_2PI;

		// check for overlaps
		for (int i = mCount; i--;)
		{
			if (angles[i] >= angles[i + 1])
			{
				return false;
			}
		}

		float scoreAtEnd = CalculateScoreForAngles(angles);
		if (scoreAtEnd > score)
		{
			ResetWithAngles(mCount, &angles[0]);
			return true;
		}
	}
	return false;
}


float DivisionAdjuster::CalculateScore() const
{
	float result = 0;
	using namespace DirectX;
	XMVECTOR nextIntegral = mIntegrals[0];
	for (int i = mCount; i--;)
	{
		auto di = nextIntegral - mIntegrals[i];
		result += XMVector3Dot(di, di).m128_f32[0] / (mAngles[i + 1] - mAngles[i]);
		nextIntegral = mIntegrals[i];
	}
	return result;
}

///////////////////////////////////////////////////////////////////////////////
// private

void DivisionAdjuster::ResetAllColors()
{
	using namespace DirectX;

	mColors.resize(mCount);
	mIntegrals.resize(mCount + 1);
	mF.resize(mCount);
	mDF.resize(mCount);
	for (int i = mCount; i--;)
	{
		mIntegrals[i] = mFourier.IntegralWithoutConstantAtAngle(mAngles[i]);
	}
	mIntegrals[mCount] = mIntegrals[0];

	for (int i = 0; i < mCount; ++i)
	{
		float deltaPhi = mAngles[i + 1] - mAngles[i];
		mColors[i] = (mIntegrals[i + 1] - mIntegrals[i]) / deltaPhi;
	}
	
	ColorVector fk(mCount), fk1(mCount);
	XMVECTOR color = mColors.back();
	for (int i = 0; i < mCount; ++i)
	{
		XMVECTOR f = mFourier.ColorComponentsWithoutConstantAtAngle(mAngles[i]);
		mF[i] = f;
		fk1[i] = f - color;
		color = mColors[i];
		fk[i] = color - f;
	}

	for (int i = 0; i < mCount; ++i)
	{
		float q = XMVector3Dot(fk[i], fk[i]).m128_f32[0];
		float q1 = XMVector3Dot(fk1[i], fk1[i]).m128_f32[0];
		SpecialEquationSolver::mB[i] = q - q1;

		XMVECTOR derivativeF = mFourier.DerivativeAtAngle(mAngles[i]);
		mDF[i] = derivativeF;
		XMVECTOR deltaC = fk[i] + fk1[i];
		float deltaPhi = mAngles[i + 1] - mAngles[i];
		int prevIndex = (i + mCount - 1) % mCount;
		float prevDeltaPhi = mAngles[prevIndex + 1] - mAngles[prevIndex];
		SpecialEquationSolver::mDiagonal[i] = 2 * (q / deltaPhi + q1 / prevDeltaPhi -
			XMVector3Dot(derivativeF, deltaC).m128_f32[0]);

		int nextIndex = (i + 1) % mCount;
		SpecialEquationSolver::mNonDiagonal[i] = 2 *
			XMVector3Dot(fk1[nextIndex], fk[i]).m128_f32[0] / deltaPhi;
	}
}

void DivisionAdjuster::ResetColorAtIndex(int inIndex)
{
	int nextIndex = (inIndex + 1) % mCount;
	int prevIndex = (inIndex + mCount - 1) % mCount;
	float deltaAngle = mAngles[inIndex + 1] - mAngles[inIndex];
	float prevDeltaAngle = mAngles[prevIndex + 1] - mAngles[prevIndex];

	if (fabs(deltaAngle) < 0.05 || fabs(prevDeltaAngle) < 0.05)
	{
		OutputDebugStringA(" two equal angles? \n");
	}

	using namespace DirectX;

	SpecialEquationSolver::mB[inIndex] =
		CalculateFValueForNewAngleBetweenIndexes(mAngles[inIndex], prevIndex, nextIndex);
	mIntegrals[inIndex] = mTemporaryIntegral;
	mF[inIndex] = mTemporaryF;
	mColors[inIndex] = mTemporaryColor;
	mColors[prevIndex] = mTemporaryPrevColor;

	XMVECTOR derivative = mFourier.DerivativeAtAngle(mAngles[inIndex]);
	mDF[inIndex] = derivative;
	XMVECTOR deltaC = mColors[inIndex] - mColors[prevIndex];
	SpecialEquationSolver::mDiagonal[inIndex] =
		2 * (mTemporaryQ / deltaAngle + mTemporaryQ1 / prevDeltaAngle -
		XMVector3Dot(derivative, deltaC).m128_f32[0]);

	XMVECTOR h1 = mF[nextIndex] - mColors[inIndex];
	XMVECTOR h = mColors[inIndex] - mF[inIndex];
	SpecialEquationSolver::mNonDiagonal[inIndex] =
		2 * XMVector3Dot(h1, h).m128_f32[0] / deltaAngle;

	XMVECTOR c1f1 = mColors[nextIndex] - mF[nextIndex];
	float q = XMVector3Dot(c1f1, c1f1).m128_f32[0];
	float q1 = XMVector3Dot(h1, h1).m128_f32[0];
	SpecialEquationSolver::mB[nextIndex] = q - q1;

	deltaC = mColors[nextIndex] - mColors[inIndex];
	float nextDeltaAngle = mAngles[nextIndex + 1] - mAngles[nextIndex];
	SpecialEquationSolver::mDiagonal[nextIndex] = 2 *
		(q / nextDeltaAngle + q1 / deltaAngle - XMVector3Dot(mDF[nextIndex], deltaC).m128_f32[0]);


	XMVECTOR cmf = mF[inIndex] - mColors[prevIndex];
	XMVECTOR cmf1 = mColors[prevIndex] - mF[prevIndex];
	SpecialEquationSolver::mNonDiagonal[prevIndex] =
		2 * XMVector3Dot(cmf, cmf1).m128_f32[0] / prevDeltaAngle;

	if (mCount > 2)
	{
		int prevPrevIndex = (prevIndex + mCount - 1) % mCount;
		XMVECTOR cmf2 = mF[prevIndex] - mColors[prevPrevIndex];
		q = XMVector3Dot(cmf1, cmf1).m128_f32[0];
		q1 = XMVector3Dot(cmf2, cmf2).m128_f32[0];
		SpecialEquationSolver::mB[prevIndex] = q - q1;

		deltaC = mColors[prevIndex] - mColors[prevPrevIndex];
		float prevPrevDeltaAngle = mAngles[prevPrevIndex + 1] - mAngles[prevPrevIndex];

		if (fabs(prevPrevDeltaAngle) < 0.05)
		{
			OutputDebugStringA(" prevDelta?\n");
		}

		SpecialEquationSolver::mDiagonal[prevIndex] = 2 *
			(q / prevDeltaAngle + q1 / prevPrevDeltaAngle -
			XMVector3Dot(mDF[prevIndex], deltaC).m128_f32[0]);
	}
}

void DivisionAdjuster::RotateUp()
{
	SpecialEquationSolver::RotateUp();

	int lastIndex = mCount - 1;
	auto color = mColors[lastIndex];
	auto integral = mIntegrals[lastIndex];
	auto angle = mAngles[lastIndex];
	auto f = mF[lastIndex];
	auto df = mDF[lastIndex];
	for (int i = mCount - 1; i > 0; --i)
	{
		mColors[i] = mColors[i - 1];
		mIntegrals[i] = mIntegrals[i - 1];
		mAngles[i] = mAngles[i - 1];
		mF[i] = mF[i - 1];
		mDF[i] = mDF[i - 1];
	}
	
	mColors[0] = color;
	mIntegrals[0] = integral;
	mAngles[0] = angle;
	mF[0] = f;
	mDF[0] = df;

	mAngles[mCount] = mAngles[0] + DirectX::XM_2PI;
}

void DivisionAdjuster::RotateDown()
{
	SpecialEquationSolver::RotateDown();

	auto color = mColors[0];
	auto integral = mIntegrals[0];
	auto angle = mAngles[0];
	auto f = mF[0];
	auto df = mDF[0];
	for (int i = 1; i < mCount; ++i)
	{
		mColors[i - 1] = mColors[i];
		mIntegrals[i - 1] = mIntegrals[i];
		mAngles[i - 1] = mAngles[i];
		mF[i - 1] = mF[i];
		mDF[i - 1] = mDF[i];
	}
	int lastIndex = mCount - 1;
	mColors[lastIndex] = color;
	mIntegrals[lastIndex] = integral;
	mAngles[lastIndex] = angle;
	mF[lastIndex] = f;
	mDF[lastIndex] = df;

	mAngles[mCount] = mAngles[0] + DirectX::XM_2PI;
}

void DivisionAdjuster::RotateOnlyAnglesUp()
{
	auto endIterator = mAngles.begin() + mCount;
	std::rotate(mAngles.begin(), endIterator - 1, endIterator);
}

void DivisionAdjuster::RotateOnlyAnglesDown()
{
	auto begin = mAngles.begin();;
	std::rotate(begin, begin + 1, begin + mCount);
}

float DivisionAdjuster::CalculateFValueForNewAngleBetweenIndexes(float inAngle,
	int inPrevIndex, int inNextIndex)
{
	using namespace DirectX;
	float prevAngle = mAngles[inPrevIndex];
	float nextAngle = mAngles[inNextIndex];
	if (prevAngle >= nextAngle)
	{
		nextAngle += XM_2PI;
		if (inAngle < prevAngle)
		{
			inAngle += XM_2PI;
		}
	}
	float prevDeltaAngle = inAngle - prevAngle;
	float deltaAngle = nextAngle - inAngle;

	mTemporaryIntegral = mFourier.IntegralWithoutConstantAtAngle(inAngle);
	mTemporaryPrevColor = (mTemporaryIntegral - mIntegrals[inPrevIndex]) / prevDeltaAngle;

	mTemporaryColor = (mIntegrals[inNextIndex] - mTemporaryIntegral) / deltaAngle;

	mTemporaryF = mFourier.ColorComponentsWithoutConstantAtAngle(inAngle);
	auto c1f = mTemporaryColor - mTemporaryF;
	auto c0f = mTemporaryF - mTemporaryPrevColor;
	mTemporaryQ = XMVector3Dot(c1f, c1f).m128_f32[0];
	mTemporaryQ1 = XMVector3Dot(c0f, c0f).m128_f32[0];
	return mTemporaryQ - mTemporaryQ1;
}

float DivisionAdjuster::CalculatePartialScore(float inAngle)
{
	using namespace DirectX;
	auto integral = mFourier.IntegralWithoutConstantAtAngle(inAngle);
	auto dNext = mTemporaryNextIntegral - integral;
	auto dPrev = integral - mTemporaryPrevIntegral;
	return XMVector3Dot(dNext, dNext).m128_f32[0] / (mTemporaryNextAngle - inAngle)
		+ XMVector3Dot(dPrev, dPrev).m128_f32[0] / (inAngle - mTemporaryPrevAngle);
}

float DivisionAdjuster::CalculateScoreForAngles(const FloatVector& inAngles) const
{
	using namespace DirectX;

	ColorVector integrals(mCount + 1);
	for (int i = mCount; i--;)
	{
		integrals[i] = mFourier.IntegralWithoutConstantAtAngle(inAngles[i]);
	}
	integrals[mCount] = integrals[0];

	float score = 0;
	for (int i = mCount; i--;)
	{
		auto d = integrals[i + 1] - integrals[i];
		score += XMVector3Dot(d, d).m128_f32[0] / (inAngles[i + 1] - inAngles[i]);
	}
	return score;
}

void DivisionAdjuster::CalculateOptimalSubdivisions(
	ScoreAndMaskVector& outOptimalScoreAndMasks) const
{
	assert(mCount >= 2);

	// any initial negative score is suitable
	static const ScoreAndMask initialData = { -1.0f, 0 };
	outOptimalScoreAndMasks.assign(mCount - 1, initialData);

	using namespace DirectX;

	// temporary storage for terms like: (F(k+1) - F(k))^2 / (phi(k+1) - phi(k))
	FloatVector scoreTerms(mCount * mCount);
	for (int i = mCount; i--;)
	{
		for (int j = i; j--;)
		{
			auto deltaI = mIntegrals[i] - mIntegrals[j];
			float q = XMVector3Dot(deltaI, deltaI).m128_f32[0];
			float deltaAngle = mAngles[i] - mAngles[j];

			// every two points break the circle into two arcs
			scoreTerms[i + mCount * j] = q / deltaAngle;
			scoreTerms[j + mCount * i] = q / (XM_2PI - deltaAngle);
		}
	}

	// the algorithm would not work if the number of segments
	// exceeds the number of bits in the MaskType
	assert(mCount < 8 * sizeof(MaskType));
	
	// the algorithms uses a lot of memory as 3-D array [i][j][bitCount]
	ScoreAndMaskVector vv(mCount * mCount * mCount);

	for (int i = 1; i < mCount; ++i)
	{
		for (int j = i; j--;)
		{
			float overlapScore = scoreTerms[j + mCount * i];
			ScoreAndMask* sm = &vv[mCount * (j + mCount * i)];
			sm->score = scoreTerms[i + mCount * j];
			sm->mask = (1 << i) | (1 << j);
			float score = sm->score + overlapScore;
			ScoreAndMask& resultSM = outOptimalScoreAndMasks[0];
			if (score > resultSM.score)
			{
				resultSM.score = score;
				resultSM.mask = sm->mask;
			}
	
			for (int bc = i - j - 1; bc--; )
			{
				ScoreAndMask element = initialData;
				for (int iSmaller = j + bc + 1; iSmaller < i; ++iSmaller)
				{
					ScoreAndMask& sm = vv[bc + mCount * (j + mCount * iSmaller)];
					float score = sm.score + scoreTerms[i + mCount * iSmaller];
					if (score > element.score)
					{
						element.score = score;
						element.mask = sm.mask;
					}
				}
				element.mask |= 1 << i;
				vv[bc + 1 + mCount * (j + mCount * i)] = element;

				float score = element.score + overlapScore;
				ScoreAndMask& resultSM = outOptimalScoreAndMasks[bc + 1];
				if (score > resultSM.score)
				{
					resultSM.score = score;
					resultSM.mask = element.mask;
				}
			}
		}
	}
}

void DivisionAdjuster::RawAdjustTowardsHigherScore(float& ioCurrentScore,
	float inHigherScore)
{
	while (RawAdjustAllAngles())
	{
		float oldScore = ioCurrentScore;
		ioCurrentScore = CalculateScore();
		float gap = inHigherScore - ioCurrentScore;
		if (gap <= 0 || (ioCurrentScore - oldScore) < 0.1f * gap)
		{
			break;
		}
	}
}
